#pragma once

#include <memory>

#include "CoordinateSys.h"
#include "CartesianCRD.hpp"
#include "GeodeticCRD.hpp"
#include "TopoCentricCRD.hpp"

namespace ns_Position
{
    class TopoCentricPolarCRD : public CoordinateSys
    {
    public:
        using PositionType = double;

    public:
        TopoCentricPolarCRD(const CartesianCRD &central)
            : _central(central)
        {
        }

        TopoCentricPolarCRD(PositionType range, PositionType azimuth, PositionType elevation, const CartesianCRD &central)
            : _range(range),
              _azimuth(azimuth),
              _elevation(elevation),
              _central(central)
        {
        }

        bool ToCartesianCRD(CartesianCRD *out) const override
        {
            // TODO
            return false;
        }

        bool FromCartesianCRD(const CartesianCRD &ref) override
        {
            std::unique_ptr<TopoCentricCRD> tmp(new TopoCentricCRD(_central));
            tmp->FromCartesianCRD(ref);

            this->FromTopoCentricCRD(tmp.get());

            return true;
        }

        PositionType GetRange() const
        {
            return _range;
        }

        PositionType GetAzimuth() const
        {
            return _azimuth;
        }

        PositionType GetElevation() const
        {
            return _elevation;
        }
        
        CartesianCRD GetCentral() const
        {
            return _central;
        }

    private:
        PositionType _range;     // 卫星到接收站的距离
        PositionType _azimuth;   // 方位角
        PositionType _elevation; // 高度角

        CartesianCRD _central;

        TopoCentricPolarCRD *FromTopoCentricCRD(TopoCentricCRD *input)
        {
            TopoCentricCRD::PositionType N = input->GetNorthing();
            TopoCentricCRD::PositionType E = input->GetEasting();
            TopoCentricCRD::PositionType U = input->GetUpping();

            _range = Mathf::Sqrt(N * N + E * E + U * U);
            _azimuth = Mathf::Arctan(N, E);
            _elevation = Mathf::Arcsin(U / _range);

            return this;
        }
    };
}